/*
 * @Author: your name
 * @Date: 2021-05-01 15:45:59
 * @LastEditTime: 2021-05-06 19:06:08
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \gcxz\Middlewares\Device\MOTOR.cpp
 */
#include "motor.h"

void motor::setTarget(float target){
	target_speed = target;
	pid.Target = target_speed;
}

void motor::control(){
	// pid.Adjust();
	pid.openloop();
	setPWM(pid.Out);
}

void motor::init(){
	ftm_pwm_init(ftm, ch_positive, pwm_frequency, 0);	
	ftm_pwm_init(ftm, ch_negative, pwm_frequency, 0);
}

/**
 * @description: TODO：对编码器数据进行处理
 * @param {*}
 * @return {*}
 */
void motor::handleEncoder(){
	encoder = encoders[MAX_ENCODERS_SIZE - 1];
}

void motor::updateSpeed(int _encoder){
	for(int i = 0; i < MAX_ENCODERS_SIZE - 1; i++)
		encoders[i] = encoders[i + 1];

	encoders[MAX_ENCODERS_SIZE - 1] = _encoder;
	
	handleEncoder();
	
	current_speed = encoder * encode_to_speed;
	pid.Current = current_speed;
}

void motor::setPWM(int val){
	pwm_out = val;
	if(pwm_out >= 0){
		ftm_pwm_duty(ftm, ch_positive, pwm_out);
		ftm_pwm_duty(ftm, ch_negative, 0);
	} else{
		ftm_pwm_duty(ftm, ch_positive, 0);
		ftm_pwm_duty(ftm, ch_negative, -pwm_out);		
	}
}
